/*
 * @file position_target_server.h
 *
 * @brief the robot will search for a ball near the robot in this state, reject balls that are far away.
 * @author Michiel Hegemans
 * @author Peter Evers
 *
 * @date December 2012
 */

#include <ar_pose/ARMarkers.h>

#include "servers/base_server.h"
#include "rp_states/PositionTargetAction.h"

class PositionTargetAction : public BaseServer {
private:
	actionlib::SimpleActionServer<rp_states::PositionTargetAction> as_;

	ros::Subscriber marker_sub_;
	ar_pose::ARMarker closest_marker_;

	bool marker_found_;
	int marker_search_id_;
	double trunk_distance_;
	double stop_distance_;
	double thresholdx_;
	double thresholdz_;

	rp_states::PositionTargetFeedback feedback_;
	rp_states::PositionTargetResult result_;

	ros::Time marker_lost_time_;

	/**
	 * Callback receiving the markers.
	 */
	void markerCallback(const ar_pose::ARMarkers::ConstPtr &msg);
public:
	PositionTargetAction(std::string name);
	~PositionTargetAction();

	/**
	 * Subscribe to topics and get the trunk parameter.
	 */
	void init();

	/**
	 * Execute goal, position robot.
	 * @param	goal	goal containing the marker to position the robot.
	 */
	void execute(const rp_states::PositionTargetGoalConstPtr &goal);

	/**
	 * Get the closest marker.
	 */
	ar_pose::ARMarker getClosestMarker();

	/**
	 * Set the closest marker (for testing purpose).
	 * @param	marker	the marker.
	 */
	void setClosestMarker(ar_pose::ARMarker marker);

	/**
	 * Check if a marker is found.
	 */
	bool isMarkerFound();

	/**
	 * Set the found state of the marker (for testing purpose).
	 * @param	found	boolean with the state of the marker (true is found, false is not found duh!).
	 */
	void setMarkerFound(bool state);

};

